#pragma once
#include "basic.h"
#include "SINS.h"

#pragma pack(push, 16)
class CLCKF // class: loose coupling kf 15-states
{
private:
    double tk_1, dt; 
    CMat15 Pkk_1;
    CVec15 xkk_1, TDk;
    void predict_xkpk(const double *xk, const double *Pk,
                         const double *Qt, const double *Cnb,
                         const double *Mpvv, const double *fn,
                         const double *betaGyr, const double *betaAcc, double dt,
                         double *xkk_1, double *Pkk_1);

public:
    // EIGEN_MAKE_ALIGNED_OPERATOR_NEW   
    double tk;
    CSINS  sins;
    CVec3  outLever;
    CAvptRingBuffer avpt;
    CVec3  betaGyr, betaAcc;
    
    CVec15 xk, Kk, Qt, Rk, xkMax, pkMax, pkMin;
    CVec15 innov, innov_var, rho, lambda;  // for Soft Chi-Square Test
    CMat15 Pk;
    
    explicit CLCKF(const CVec3 &att0 = O31, const CVec3 &vn0 = O31, const CVec3 &pos0 = O31);
    ~CLCKF() = default;

    void init(const CVec3 &att0, const CVec3 &vn0, const CVec3 &pos0, double tk);
    void sins_update(const double *gyr, const double *acc, double tk, int nSamples = NSAMPLES);
    void time_update(double tk);
    
    void gnssMeas_update(double tk, const CVec3 &vn, const CVec3 &pos, bool isHgtValid = false, const double *dualAntennaYaw = nullptr);
    void magnMeas_update(double tk, const CVec3 &Magn);
    void baroMeas_update(double tk, double baroHgt);
    
    virtual void feedback(uint32_t bits);
    void clear();

    /* kalman filter health monitoring*/
    bool requestToResetFilter;
    uint32_t resetFilterCnt;
    uint32_t pkNonPositiveDefiniteCnt;      // P阵非正定计数
    double obs_tGnssYaw[2], obs_tGnss[7];   // for example: obs_tGnssYaw = {tk, dualAntennaYaw};
    double obs_tMagYaw[2], obs_tBaroHgt[2];

    void resetFilter(void);
    void resetAtt(double pit, double pitNoiseStd, double rol, double rolNoiseStd, double yaw, double yawNoiseStd);
    void resetYaw(double yaw, double yawNoiseStd);
    void resetVelNE(double velN, double vNNoiseStd, double velE, double vENoiseStd);
    void resetVelU(double velU, double vUNoiseStd);
    void resetPosNE(double posN, double posNNoiseStd, double posE, double posENoiseStd);
    void resetPosU(double posU, double posUNoiseStd);
};
#pragma pack(pop)

